/******************************************************************************/
/*                                                                            */
/*                       Copyright (c) HANBACK ELECTRONICS                    */
/*                              All rights reserved.                          */
/*                                                                            */
/*                             http://www.hanback.com                         */
/*                                                                            */
/******************************************************************************/

/******************************************************************************/
/*                                                                            */
/*============================================================================*/
/* Permission to use, copy, modify, and distribute this software and its      */
/* documentation are reserved by above authors and Hanback electronics.       */
/* The above copyright notice and authors must be described in this software. */
/*============================================================================*/
/*                                                                            */
/******************************************************************************/

module Hanback_TreeRoutingM {

  provides {
	interface StdControl;
	interface AMSend as SendFromAPP;
	interface Receive as RecvToAPP;  
  }
  uses {
    interface Timer<TMilli>;
    interface ParameterInit<uint16_t> as SeedInit;
    interface Random;
    interface SplitControl as CommControl;
    interface CC2430Control;
    interface AMSend as SendData;
    interface Receive as RecvData;
    interface AMSend as SendBeacon;
    interface Receive as RecvBeacon; 
    interface SCSuartDBG;
  }

}implementation{

  message_t	TXFrame;
  message_t	BFrame; 
  message_t	RXFrame;
  message_t	ForwardingFrame;
  uint16_t	myHopNumber;
  uint16_t	NextAddress;
  uint16_t	TimeCount;
  uint16_t	TX_Type;
  uint8_t	SeqNum_;
  uint8_t	NextAddress_TableIndex_;
  uint8_t	Check_Routing_Error;
  NeighborTable	NTableList[NumNeighborTable];
  uint8_t	MsgBuff[64];

  void updateRoutingTable();
  void processNextAddress();
  task void TransmitBeacon();
  task void TransmitBeaconRequest();
  task void ForwadingDataFrame();

  command error_t StdControl.start() {
    call SeedInit.init(TOS_NODE_ID);
    atomic {
        uint8_t i, random_num = (uint8_t) call Random.rand16();

	TimeCount = 0;
	SeqNum_   = (uint8_t) (random_num%0xFF);
	TX_Type   = Snoop_Null;
	NextAddress_TableIndex_ = 0;
	Check_Routing_Error = 0;
	for ( i=0 ; i<NumNeighborTable ; i++)
		NTableList[i].Naddr = UnkownAddress;
	
	if (TOS_NODE_ID==SinkAddress){
		myHopNumber = 0;
		NextAddress = SinkAddress;
	}else{
		myHopNumber = UnkownNum;
		NextAddress = UnkownAddress;
	}
    }
    call CommControl.start();
    return SUCCESS;
  }

  event void CommControl.startDone(error_t error) {
	
	///////////////////////////////////////////////////////////
	//                 Channel of CC2431
	//  Channel Ranges: 11 ~ 26
	///////////////////////////////////////////////////////////
        uint8_t myChannel = 15;
        call CC2430Control.TunePreset (myChannel);
	///////////////////////////////////////////////////////////
	if(TOS_NODE_ID==SinkAddress)
		post TransmitBeacon();
	else
		post TransmitBeaconRequest();
	call Timer.startPeriodic(BeaconInterval);
  }
  
  command error_t StdControl.stop() {call CommControl.stop();return SUCCESS;}
  event void CommControl.stopDone(error_t error) {}

//////////////////////////////////////////////////////////////////////

  command error_t SendFromAPP.send(am_addr_t addr, message_t* msg, uint8_t len){
	processNextAddress();
	
	if (NextAddress != UnkownAddress){
		NWKFrame Route_M;
	
		Route_M.FrameControl = GeneralDataFrame;
		Route_M.DstAddr = addr;
		Route_M.SrcAddr = TOS_NODE_ID;
		Route_M.Radius  = MaxHopNum;
		Route_M.SeqNum  = SeqNum_++;

		Route_M.Dst2_for_multihop = UnkownAddress;
		Route_M.Dst3_for_multihop = UnkownAddress;
		memcpy(Route_M.UpperData, call SendData.getPayload(msg), len);
		memcpy(call SendData.getPayload(&TXFrame), (uint8_t *)&Route_M, sizeof(NWKFrame));

		//printf(" ========= %d tx send data to %d\n", TOS_LOCAL_ADDRESS, NextAddress);
		if (call SendData.send (NextAddress, &TXFrame, sizeof(NWKFrame)) == SUCCESS)
		{
			atomic TX_Type = GeneralDataFrame;
			sprintf(MsgBuff, "%d tx data to %d\r\n", TOS_NODE_ID, NextAddress);
			if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));
		}else{
			return FAIL;
		}

	}else{
		return FAIL;
	}

	return SUCCESS;
  }

  command error_t SendFromAPP.cancel(message_t* msg){
	return call SendData.cancel(msg);
  }

  command uint8_t SendFromAPP.maxPayloadLength(){
	return call SendData.maxPayloadLength();
  }

  command void* SendFromAPP.getPayload(message_t* msg){
	return call SendData.getPayload(msg);
  }

/////////////////////////////////////////////////////////////////////////////////////

  event void SendData.sendDone(message_t* msg, error_t error){

	if (TX_Type == GeneralDataFrame)
		signal SendFromAPP.sendDone (msg, error);
		
	if (TX_Type==GeneralDataFrame || TX_Type==ForwardDataFrame)
	{
		if (error != SUCCESS)
		{
			uint16_t previous_Next_address;
			Check_Routing_Error++;
			if (Check_Routing_Error > MAX_FAIL_NUM) {
				NTableList[NextAddress_TableIndex_].Naddr = UnkownAddress;
				Check_Routing_Error = 0;
				previous_Next_address = NextAddress;
				myHopNumber = UnkownNum;
				NextAddress = UnkownAddress;
					
				sprintf(MsgBuff, "%d reprocess Nextaddr\r\n", TOS_NODE_ID);
				if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));

				processNextAddress();
			}else{
				sprintf(MsgBuff, "%d sendDone FAIL! to %d\r\n", TOS_NODE_ID, NextAddress);
				if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));
			}

		}else{
			Check_Routing_Error = 0;
		}
	}
	TX_Type = Snoop_Null;
  }

  event void SendBeacon.sendDone(message_t* msg, error_t error) {TX_Type = Snoop_Null;}

  event message_t* RecvData.receive(message_t* msg, void* payload, uint8_t len) {
	NWKFrame RcvMsg;

	memcpy((uint8_t *)&RcvMsg, call SendData.getPayload(msg), sizeof(NWKFrame));
	TX_Type = Snoop_Null;

	//sprintf(MsgBuff,  "--- %d recv DATAFRAME[%d] from %d \r\n", TOS_NODE_ID, RcvMsg.FrameControl, RcvMsg.SrcAddr);
	//if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));

	if (RcvMsg.FrameControl==GeneralDataFrame || RcvMsg.FrameControl==ForwardDataFrame){

		if (TOS_NODE_ID == SinkAddress) {
			signal RecvToAPP.receive (msg, RcvMsg.UpperData, sizeof(NWKFrame));

		} else if (NextAddress != UnkownAddress) {
			//In this case, nodes try to forward 'm' packet to nextaddress.
			atomic memcpy (call SendData.getPayload(&ForwardingFrame), (uint8_t *)&RcvMsg, sizeof(NWKFrame));
			post ForwadingDataFrame();
		}
	}
	return msg;
  }

  event message_t* RecvBeacon.receive(message_t* msg, void* payload, uint8_t len) {
	BeaconFrame RcvMsg;
	
	memcpy((uint8_t *)&RcvMsg, call SendData.getPayload(msg), sizeof(BeaconFrame));

	sprintf(MsgBuff, " --- %d recv Beacon[%d] from %d (hop:%d, next:%d) \r\n", TOS_NODE_ID, RcvMsg.FrameControl, RcvMsg.SrcAddr, RcvMsg.HopNum_from_Sink, NextAddress);
	if (TOS_NODE_ID!= SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));

	if (RcvMsg.FrameControl ==  BeaconBySink || RcvMsg.FrameControl ==  BeaconByNode) {
		
		if (TOS_NODE_ID >= 5 && RcvMsg.SrcAddr==0)	// for tentative multi-hop routing (1~4 only)
			return msg;
				
		if (TOS_NODE_ID >= 9 && RcvMsg.SrcAddr<=4)	// 2nd layer hop (5~8 only)
			return msg;
			
		if (TOS_NODE_ID != SinkAddress){
			atomic memcpy (&RXFrame, msg, sizeof(RXFrame));
			updateRoutingTable();
		}

	}else if (RcvMsg.FrameControl==BeaconRequest){
		post TransmitBeacon();
	}

	return msg;
  }
  
  event void Timer.fired() {
	if (TOS_NODE_ID != SinkAddress){
		processNextAddress();
	}
  	post TransmitBeacon();
	TimeCount += (BeaconInterval/1000);
  }

  task void TransmitBeacon() {
	if (NextAddress != UnkownAddress) {
		BeaconFrame BF;

		if (TOS_NODE_ID==SinkAddress){
			BF.FrameControl = BeaconBySink;
			BF.SrcAddr = TOS_NODE_ID;
			BF.HopNum_from_Sink = 0;
		}else{
			BF.FrameControl = BeaconByNode;
			BF.SrcAddr = TOS_NODE_ID;
			BF.HopNum_from_Sink = myHopNumber;
		}
		
		memcpy(call SendBeacon.getPayload(&BFrame), &BF, sizeof(BeaconFrame));
		if (call SendBeacon.send (AM_BROADCAST_ADDR, &BFrame, sizeof(BeaconFrame)) == SUCCESS) {
			TX_Type = BF.FrameControl;
			sprintf(MsgBuff, "%d tx Beacon (myhop:%d, next:%d)\r\n", TOS_NODE_ID, BF.HopNum_from_Sink, NextAddress);
			if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));
		}
	}
  }

  task void TransmitBeaconRequest() {
	BeaconFrame BF;

	BF.FrameControl = BeaconRequest;
	BF.SrcAddr = TOS_NODE_ID;
	BF.HopNum_from_Sink = 0;
	
	memcpy(call SendBeacon.getPayload(&BFrame), &BF, sizeof(BeaconFrame));
	if (call SendBeacon.send (AM_BROADCAST_ADDR, &BFrame, sizeof(BeaconFrame)) == SUCCESS) {
		TX_Type = BF.FrameControl;
		sprintf(MsgBuff, "%d tx Beacon Request\r\n", TOS_NODE_ID);
		if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));
	}
  }

  task void ForwadingDataFrame()
  {
	NWKFrame NWKF;
	error_t result;

	memcpy(&NWKF, call SendData.getPayload(&ForwardingFrame), sizeof(NWKFrame));
	if (NWKF.Radius!=0)
		NWKF.Radius--;
	
	if ((NWKF.Radius==0) || (NextAddress==UnkownAddress))
		return;

	if (NWKF.Dst2_for_multihop == UnkownAddress){
		NWKF.Dst2_for_multihop = TOS_NODE_ID;
	}else{
		NWKF.Dst3_for_multihop = NWKF.Dst2_for_multihop;
		NWKF.Dst2_for_multihop = TOS_NODE_ID;
	}
	NWKF.FrameControl = ForwardDataFrame;
	
	memcpy(call SendData.getPayload(&ForwardingFrame), &NWKF, sizeof(NWKFrame));
	result = call SendData.send (NextAddress, &ForwardingFrame, sizeof(NWKFrame));
	if (result == SUCCESS){
		TX_Type = ForwardDataFrame;
		sprintf(MsgBuff, "%d forward %d to %d\r\n", TOS_NODE_ID, NWKF.SrcAddr, NextAddress);
		if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));

	}else{
		sprintf(MsgBuff, "%d forward fail[%d] %d(TTL:%d) to %d\r\n", TOS_NODE_ID, result, NWKF.SrcAddr, NWKF.Radius, NextAddress);
		if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));
	}
  }

  void updateRoutingTable() // this functions is called by Tree_receive.receive()
  {
	BeaconFrame BF;
	uint8_t RX_RSSI, RX_LQI, RX_HopCount, i, insert_index=0, MaxHopCount=0, MinLQI=0xFF;
	uint16_t SrcAddress;
	cc2430_metadata_t* metadata;
	char MinRSSI = 127;

	memcpy(&BF, call SendData.getPayload(&RXFrame), sizeof(BeaconFrame));
	RX_HopCount = BF.HopNum_from_Sink;
	SrcAddress = BF.SrcAddr;

	metadata = (cc2430_metadata_t*) (&RXFrame.metadata);
	RX_RSSI = metadata->rssi; 
	RX_LQI  = metadata->lqi; 

	// search empty entry or wrost entry
	for ( i=0 ; i<NumNeighborTable ; i++ )
	{
		if (NTableList[i].Naddr == UnkownAddress || NTableList[i].Naddr == SrcAddress) {
			insert_index = i;
			break;

		} else if (MaxHopCount <= NTableList[i].HopNum_from_Sink){
			if (MinLQI >= NTableList[i].LQI) {
				if (MinRSSI >= (char)NTableList[i].RSSI)
				{
					MaxHopCount = NTableList[i].HopNum_from_Sink;
					MinLQI  = NTableList[i].LQI;
					MinRSSI = NTableList[i].RSSI;
					insert_index = i;
				}
			}
		}
	}

	NTableList[insert_index].Naddr	= SrcAddress;
	NTableList[insert_index].HopNum_from_Sink = RX_HopCount;
	NTableList[insert_index].RSSI	= RX_RSSI;
	NTableList[insert_index].LQI	= RX_LQI;	
	NTableList[insert_index].Insert_time = TimeCount;

	sprintf(MsgBuff, " %d updateR(S:%d,I:%d,H%d)Next:%d \r\n", TOS_NODE_ID, SrcAddress, insert_index, RX_HopCount, NextAddress);
	if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));

	if(SrcAddress == SinkAddress && BF.FrameControl==BeaconBySink){
		NextAddress = SinkAddress;
		myHopNumber = 1;
		NextAddress_TableIndex_ = insert_index;
	}
  }
  
  void processNextAddress() // this functions is called by Timer.fired()
  {
	uint8_t i, NextAddress_index=0xFF, MinHopCount=0xFF, MaxLQI=0x00;
	char MaxRSSI = -127;

	// search empty entry or wrost entry
	for ( i=0 ; i<NumNeighborTable ; i++ )
	{
		if (NTableList[i].Naddr == SinkAddress) {
			NextAddress = SinkAddress;
			myHopNumber = 1;
			NextAddress_TableIndex_ = i;
			return;
		} else if (NTableList[i].Naddr != UnkownAddress) {
			if (MinHopCount >= NTableList[i].HopNum_from_Sink) {
				
				if (MaxLQI <= NTableList[i].LQI) {

					if (MaxRSSI <= (char)NTableList[i].RSSI)
					{
						MinHopCount = NTableList[i].HopNum_from_Sink;
						MaxLQI  = NTableList[i].LQI;
						MaxRSSI = NTableList[i].RSSI;
						NextAddress_index = i;
					}
				}
			}
		}
	}

	if (NextAddress_index!=0xFF)
	{
		NextAddress = NTableList[NextAddress_index].Naddr;
		myHopNumber = NTableList[NextAddress_index].HopNum_from_Sink + 1;
		NextAddress_TableIndex_ = NextAddress_index;
		if (FOR_DEMO) {
			// follow two lines may dinamicaly change topology
			NTableList[NextAddress_index].LQI = 0;
			NTableList[NextAddress_index].RSSI = -127;
		}
	}else{
		NextAddress = UnkownAddress;
		myHopNumber = UnkownAddress;
		NextAddress_TableIndex_ = 0;
	}

	sprintf(MsgBuff, " %d process (hop%d) New Next %d \r\n", TOS_NODE_ID, myHopNumber, NextAddress);
	if (TOS_NODE_ID != SinkAddress) call SCSuartDBG.UARTSend(MsgBuff, strlen(MsgBuff));
  }


  command void* RecvToAPP.getPayload(message_t* msg, uint8_t* len){
	return call RecvData.getPayload(msg, len);
  }
  command uint8_t RecvToAPP.payloadLength(message_t* msg){
	return call RecvData.payloadLength(msg);
  }

}
